﻿using UnityEngine;
using System;
using System.Collections;
using Kinect2;
using Kinect2.Face;

public class FaceBasic : MonoBehaviour {

	KinectSensor kinectSensor;
	CoordinateMapper coordinateMapper;
	ColorFrameReader colorFrameReader;
	BodyFrameReader bodyFrameReader;
	FaceFrameSource[] faceFrameSources;
	FaceFrameReader[] faceFrameReaders;
	FaceFrameResult[] faceFrameResults;
	Body[] bodies;
	Texture2D texture, fill;
	GameObject status;
	
    int bodyCount;
    int displayWidth, displayHeight;
	const float TextLayoutOffsetX = -0.1f;
	const float TextLayoutOffsetY = -0.15f;
	const double FaceRotationIncrementInDegrees = 5.0;
	string textFaceNotTracked = "No bodies or faces are tracked ...";
	bool drawFaceResult = false;

	void Start () 
	{
		kinectSensor = KinectSensor.GetDefault();
		coordinateMapper = kinectSensor.CoordinateMapper;
            
        FrameDescription frameDescription = this.kinectSensor.ColorFrameSource.FrameDescription;
        displayWidth = frameDescription.Width;
        displayHeight = frameDescription.Height;

        texture = new Texture2D(displayWidth, displayHeight, TextureFormat.BGRA32, false);


		//colorFrameReader = kinectSensor.ColorFrameSource.OpenReader();
       // colorFrameReader.FrameArrived += Reader_ColorFrameArrived;

		bodyFrameReader = kinectSensor.BodyFrameSource.OpenReader();
        bodyFrameReader.FrameArrived += Reader_BodyFrameArrived;
        bodyCount = kinectSensor.BodyFrameSource.BodyCount;
        bodies = new Body[bodyCount];			

		FaceFrameFeatures faceFrameFeatures = FaceFrameFeatures.BoundingBoxInColorSpace
			| FaceFrameFeatures.PointsInColorSpace
			| FaceFrameFeatures.RotationOrientation
			| FaceFrameFeatures.Happy
			| FaceFrameFeatures.RightEyeClosed
			| FaceFrameFeatures.LeftEyeClosed
			| FaceFrameFeatures.MouthOpen
			| FaceFrameFeatures.MouthMoved
			| FaceFrameFeatures.LookingAway
			| FaceFrameFeatures.Glasses
			| FaceFrameFeatures.FaceEngagement;

        faceFrameSources = new FaceFrameSource[bodyCount];
        faceFrameReaders = new FaceFrameReader[bodyCount];
            
		for (int i = 0; i < bodyCount; i++){
			faceFrameSources[i] = new FaceFrameSource(kinectSensor, 0, faceFrameFeatures);
			faceFrameReaders[i] = faceFrameSources[i].OpenReader();
            faceFrameReaders[i].FrameArrived += Reader_FaceFrameArrived;
		}

        faceFrameResults = new FaceFrameResult[bodyCount];
        kinectSensor.IsAvailableChanged += Sensor_IsAvailableChanged;
		kinectSensor.Open ();

        InitializeComponent();
	}

	void Update () 
	{
        EventRouter.Check();
	}

	void OnGUI()
	{
		GUI.color = Color.yellow;
		GUI.skin.box.alignment = TextAnchor.UpperLeft;

		if (drawFaceResult) 
		{
			for (int i = 0; i < bodyCount; i++)
			{
				if (faceFrameSources[i].IsTrackingIdValid
				    && faceFrameResults[i] != null)
					DrawFaceFrameResults(i, faceFrameResults[i]);
			}
		}
		else
		{
			Vector2 pos = ColorSpaceToScreenVector (0, 0);
			GUI.Box (new Rect (pos.x, pos.y, 240, 20), textFaceNotTracked);
		}

	}

	void OnApplicationQuit()
    {
        for (int i = 0; i < bodyCount; i++)
        {
            if (faceFrameReaders[i] != null)
                faceFrameReaders[i].Dispose();
            faceFrameReaders[i] = null;

            if (faceFrameSources[i] != null)
                faceFrameSources[i].Dispose();
            faceFrameSources[i] = null;
        }

        if (bodyFrameReader != null)
            bodyFrameReader.Dispose();
        bodyFrameReader = null;

        if (colorFrameReader != null)
            colorFrameReader.Dispose();
        colorFrameReader = null;
        
		if (kinectSensor != null)
			kinectSensor.Close ();
        kinectSensor = null;
	}

    void InitializeComponent()
    {
        GameObject color = GameObject.Find("color");
        color.guiTexture.texture = texture;
        status = GameObject.Find("status");
        fill = new Texture2D(1, 1);
        fill.SetPixel(0, 0, Color.yellow);
        fill.Apply();
        status = GameObject.Find("status");
        status.guiText.text = kinectSensor.IsAvailable ? "Running" : "No ready Kinect found!";
    }

    void Reader_FaceFrameArrived(object sender, FaceFrameArrivedEventArgs e)
    {
        using (FaceFrame faceFrame = e.FrameReference.AcquireFrame())
        {
            if (faceFrame != null)
            {
                int index = GetFaceSourceIndex(faceFrame.FaceFrameSource);

                if (ValidateFaceBoxAndPoints(faceFrame.FaceFrameResult))
                    faceFrameResults[index] = faceFrame.FaceFrameResult;
                else
                    faceFrameResults[index] = null;
                
            }
        }
    }

    void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
    {
        using (var bodyFrame = e.FrameReference.AcquireFrame())
        {
            if (bodyFrame != null)
            {
                bodyFrame.GetAndRefreshBodyData(bodies);
                drawFaceResult = false;
                for (int i = 0; i < bodyCount; i++)
                {
                    if (faceFrameSources[i].IsTrackingIdValid)
                    {
                        if (faceFrameResults[i] != null)
                        {
                            if (!drawFaceResult)
                                drawFaceResult = true;
                        }
                    }
                    else if (bodies[i].IsTracked)
                        faceFrameSources[i].TrackingId = bodies[i].TrackingId;
                }
            }
        }
    }

    void Sensor_IsAvailableChanged(object sender, IsAvailableChangedEventArgs e)
    {
        status.guiText.text = kinectSensor.IsAvailable ? "Running" : "Kinect not available!";
    }

    void Reader_ColorFrameArrived(object sender, ColorFrameArrivedEventArgs e)
    {
        using (ColorFrame colorFrame = e.FrameReference.AcquireFrame())
        {
            if (colorFrame != null)
            {
                FrameDescription colorFrameDescription = colorFrame.FrameDescription;
                using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer())
                {
                    if ((colorFrameDescription.Width == texture.width)
                        && (colorFrameDescription.Height == texture.height))
                    {
                        int width = colorFrameDescription.Width;
                        int height = colorFrameDescription.Height;
                        byte[] buffer = new byte[width * height * 4];
                        colorFrame.CopyConvertedFrameDataToArray(buffer, ColorImageFormat.Rgba);

                        byte[] data = new byte[width * height * 4];
                        for (int j = 0; j < height; j++)
                        {
                            int dst = width * 4 * (height - 1 - j);
                            int src = width * 4 * j;
                            Array.Copy(buffer, src, data, dst, width * 4);
                        }
                        texture.LoadRawTextureData(data);
                        texture.Apply();
                    }
                }
            }
        }
    }

    int GetFaceSourceIndex(FaceFrameSource faceFrameSource)
    {
        int index = -1;

        for (int i = 0; i < bodyCount; i++)
        {
            if (faceFrameSources[i] == faceFrameSource)
            {
                index = i;
                break;
            }
        }

        return index;
    }

    bool ValidateFaceBoxAndPoints(FaceFrameResult faceResult)
    {
        bool isFaceValid = faceResult != null;

        if (isFaceValid)
        {
            var faceBox = faceResult.FaceBoundingBoxInColorSpace;
            isFaceValid = (faceBox.Right - faceBox.Left) > 0 &&
                          (faceBox.Bottom - faceBox.Top) > 0 &&
                          faceBox.Right <= displayWidth &&
                          faceBox.Bottom <= displayHeight;

            if (isFaceValid)
            {
                var facePoints = faceResult.FacePointsInColorSpace;
                if (facePoints != null)
                {
                    foreach (PointF pointF in facePoints.Values)
                    {
                        bool isFacePointValid = pointF.X > 0.0f &&
                                                pointF.Y > 0.0f &&
                                                pointF.X < this.displayWidth &&
                                                pointF.Y < this.displayHeight;

                        if (!isFacePointValid)
                        {
                            isFaceValid = false;
                            break;
                        }
                    }
                }
            }
           
        }

        return isFaceValid;
    }


	void DrawFaceFrameResults(int faceIndex, FaceFrameResult faceResult)
	{
		var faceBoxSource = faceResult.FaceBoundingBoxInColorSpace;
		Vector2 leftTop = ColorSpaceToScreenVector (faceBoxSource.Left, faceBoxSource.Top);
		Vector2 rightBottom = ColorSpaceToScreenVector (faceBoxSource.Right, faceBoxSource.Bottom);
		GUI.Box (new Rect(leftTop.x, leftTop.y, rightBottom.x - leftTop.x, rightBottom.y - leftTop.y), "");

		foreach (PointF pointF in faceResult.FacePointsInColorSpace.Values)
		{
			Vector2 p = ColorSpaceToScreenVector(pointF.X, pointF.Y);
			GUI.DrawTexture (new Rect(p.x - 1, p.y - 1, 3, 3), fill);
		}

		Vector2 faceTextLayout;
		string faceText = string.Empty;
		if (faceResult.FaceProperties != null)
		{
			foreach (var item in faceResult.FaceProperties)
				faceText += string.Format("{0} : {1}\n",
				                          item.Key.ToString(),
				                          item.Value.ToString());   
		}
		int pitch, yaw, roll;
		ExtractFaceRotationInDegrees(faceResult.FaceRotationQuaternion, out pitch, out yaw, out roll);
		faceText += string.Format ("FaceYaw : {0}\nFacePitch : {1}\nFacenRoll :{2}\n", yaw, pitch, roll);

		if (GetFaceTextPositionInColorSpace (faceIndex, out faceTextLayout))
			GUI.Box (new Rect (faceTextLayout.x, faceTextLayout.y, 180, 180), faceText);
	}

	Vector2 ColorSpaceToScreenVector(float x, float y)
	{
		return new Vector2(Screen.width / 2 + (x - displayWidth / 2) / 4,
		                   Screen.height / 2 + (y - displayHeight / 2) / 4);
	}

	bool GetFaceTextPositionInColorSpace(int faceIndex, out Vector2 faceTextLayout)
	{
		faceTextLayout = new Vector2();
		bool isLayoutValid = false;
		
		Body body = this.bodies[faceIndex];
		if (body.IsTracked)
		{
			var headJoint = body.Joints[JointType.Head].Position;
			CameraSpacePoint textPoint = new CameraSpacePoint()
			{
				X = headJoint.X + TextLayoutOffsetX,
				Y = headJoint.Y + TextLayoutOffsetY,
				Z = headJoint.Z
			};
			ColorSpacePoint textPointInColor = coordinateMapper.MapCameraPointToColorSpace(textPoint);
			faceTextLayout = ColorSpaceToScreenVector(textPointInColor.X, textPointInColor.Y);
			isLayoutValid = true;                
		}
		
		return isLayoutValid;
	}

	void ExtractFaceRotationInDegrees(Kinect2.Vector4 rotQuaternion, out int pitch, out int yaw, out int roll)
	{
		double x = rotQuaternion.X;
		double y = rotQuaternion.Y;
		double z = rotQuaternion.Z;
		double w = rotQuaternion.W;
		
		double yawD, pitchD, rollD;
		pitchD = Math.Atan2(2 * ((y * z) + (w * x)), (w * w) - (x * x) - (y * y) + (z * z)) / Math.PI * 180.0;
		yawD = Math.Asin(2 * ((w * y) - (x * z))) / Math.PI * 180.0;
		rollD = Math.Atan2(2 * ((x * y) + (w * z)), (w * w) + (x * x) - (y * y) - (z * z)) / Math.PI * 180.0;
		
		double increment = FaceRotationIncrementInDegrees;
		pitch = (int)(Math.Floor((pitchD + ((increment / 2.0) * (pitchD > 0 ? 1.0 : -1.0))) / increment) * increment);
		yaw = (int)(Math.Floor((yawD + ((increment / 2.0) * (yawD > 0 ? 1.0 : -1.0))) / increment) * increment);
		roll = (int)(Math.Floor((rollD + ((increment / 2.0) * (rollD > 0 ? 1.0 : -1.0))) / increment) * increment);
	}

}
